#include "dji_motor.h"
#include "bsp_dwt.h"
#include "controller.h"

static DJIMotorInstance* DJI_Motor_Instance_List[DJI_MOTOR_MAX_NUM] = {NULL};
static uint8_t Idx = 0;

static uint8_t Motor_Enable[6] = {0};
static CANInstance send_group[6] =
{
    [0] = {.CANx = &hcan1, .tx_conf.StdId = 0x1FF, .tx_conf.IDE = CAN_ID_STD, .tx_conf.RTR = CAN_RTR_DATA, .tx_conf.DLC = 8},
    [1] = {.CANx = &hcan1, .tx_conf.StdId = 0x200, .tx_conf.IDE = CAN_ID_STD, .tx_conf.RTR = CAN_RTR_DATA, .tx_conf.DLC = 8},
    [2] = {.CANx = &hcan1, .tx_conf.StdId = 0x2FF, .tx_conf.IDE = CAN_ID_STD, .tx_conf.RTR = CAN_RTR_DATA, .tx_conf.DLC = 8},
    [3] = {.CANx = &hcan2, .tx_conf.StdId = 0x1FF, .tx_conf.IDE = CAN_ID_STD, .tx_conf.RTR = CAN_RTR_DATA, .tx_conf.DLC = 8},
    [4] = {.CANx = &hcan2, .tx_conf.StdId = 0x200, .tx_conf.IDE = CAN_ID_STD, .tx_conf.RTR = CAN_RTR_DATA, .tx_conf.DLC = 8},
    [5] = {.CANx = &hcan2, .tx_conf.StdId = 0x2FF, .tx_conf.IDE = CAN_ID_STD, .tx_conf.RTR = CAN_RTR_DATA, .tx_conf.DLC = 8},
} ;

static void SenderGroup(DJIMotorInstance* motor, CAN_Init_Config_s* config)
{
    uint8_t tx_id = config -> tx_id;
    uint8_t group_num;
    uint8_t motor_num;
    switch(motor -> motor_type)
    {
        case M3508:
        case M2006:
            if(tx_id <= 4)
            {
                motor_num = tx_id - 1;
                if(&hcan1 == config -> CANx)
                {
                    group_num = 1;
                }
                else if(&hcan2 == config -> CANx)
                {
                    group_num = 4;
                }
            }
            else
            {
                motor_num = tx_id - 5;
                if(&hcan1 == config -> CANx)
                {
                    group_num = 0;
                }
                else if(&hcan2 == config -> CANx)
                {
                    group_num = 3;
                }
            }
            config -> rx_id = 0x200 + tx_id;
            Motor_Enable[group_num] = 1;
            motor -> group_num = group_num;
            motor -> motor_num = motor_num;

            //要检查id冲突吗？
            break;
        case M6020:
            if(tx_id <= 4)
            {
                motor_num = tx_id - 1;
                if(&hcan1 == config -> CANx)
                {
                    group_num = 0;
                }
                else if(&hcan2 == config -> CANx)
                {
                    group_num = 3;
                }
            }
            else
            {
                motor_num = tx_id - 5;
                if(&hcan1 == config -> CANx)
                {
                    group_num = 2;
                }
                else if(&hcan2 == config -> CANx)
                {
                    group_num = 5;
                }
            }
            config -> rx_id = 0x204 + tx_id;
            Motor_Enable[group_num] = 1;
            motor -> group_num = group_num;
            motor -> motor_num = motor_num;

            //要检查id冲突吗？
            break;
        
        default:
            break;
    }
}

static void DJIMotorDecode(CANInstance* can_instance)
{
    uint8_t* rx_buf = can_instance -> rx_buf;
    DJIMotorInstance* motor = (DJIMotorInstance*)can_instance -> id;
    DJI_Motor_Measure_s* measure = &motor -> measure;
    measure -> last_ecd = motor -> measure.ecd;
    measure -> ecd =  (rx_buf[0] << 8) | rx_buf[1];
    //电流和速度最好在这里加个滤波，后续再进行处理
    measure -> speed = RPM_2_ANGLE_PER_SEC * ((rx_buf[2] << 8) | rx_buf[3]);
    measure -> current = (rx_buf[4] << 8) | rx_buf[5];
    measure -> temperature = rx_buf[6];
    measure -> single_round_angle = DJI_ECD_TO_ANGLE * (float)motor -> measure.ecd;

    //扣圈计算，当编码器差绝对值大于或小于编码器的一半时，视作转了一圈
    if(measure -> ecd - measure -> last_ecd > 4096)
    {
        measure -> total_round --;
    }
    else if(measure -> ecd - measure -> last_ecd < -4096)
    {
        measure -> total_round ++;
    }
    measure -> total_angle = measure -> single_round_angle + measure -> total_round * 360.0f;
}

DJIMotorInstance* DJIMotorInit(Motor_Init_Config_s* init_config)
{
    DJIMotorInstance* instance = (DJIMotorInstance*)malloc(sizeof(DJIMotorInstance));
    memset(instance, 0, sizeof(DJIMotorInstance));

    instance -> motor_type = init_config -> motor_type;
    instance -> config = init_config -> motor_control_init_config;

    PID_Init(&instance -> control.current_pid, &init_config -> motor_init_config.current_pid_config);
    PID_Init(&instance -> control.speed_pid, &init_config -> motor_init_config.speed_pid_config);
    PID_Init(&instance -> control.angle_pid, &init_config -> motor_init_config.angle_pid_config);
    instance -> control.other_angle_feedback = init_config -> motor_init_config.other_speed_feedback;
    instance -> control.other_speed_feedback = init_config -> motor_init_config.other_speed_feedback;
    //前馈类型
    //instance -> control.motor_angle_feedback = init_config -> motor_init_config.motor_angle_feedback;
    //instance -> control.motor_speed_feedback = init_config -> motor_init_config.motor_speed_feedback;

    SenderGroup(instance, &init_config -> can_init_config);

    init_config -> can_init_config.can_module_callback = DJIMotorDecode;
    init_config -> can_init_config.id = instance;
    instance -> motor_can_instance = CANInit(&init_config -> can_init_config);

    DJIMotorStart(instance);
    DJI_Motor_Instance_List[Idx ++] = instance;
    return instance;
}

void DJIMotorSetRef(DJIMotorInstance* instance, float ref)
{
    instance -> control.pid_ref = ref;
}

void DIJIMotorControl()
{
    int16_t set;
    uint8_t group_num;
    uint8_t motor_num;
    DJIMotorInstance* motor;
    DJI_Motor_Measure_s* measure;
    Motor_Control_Config_s* config;
    Motor_Control_s* control;
    float pid_measure, pid_ref;
	for(size_t i = 0; i < Idx; ++ i)
    {
        motor = DJI_Motor_Instance_List[i];
        measure = &motor -> measure;
        config = &motor -> config;
        control = &motor -> control;
        pid_ref = control -> pid_ref;
        if(motor -> config.motor_direction)
        {
            pid_ref *= -1; 
        }

        if(motor -> config.loop_type & ANGLE_LOOP)
        {
            if(config -> angle_feedback_source == MOTOR_FEED)
            {
                pid_measure = measure -> total_angle;
            }
            else
            {
                pid_measure = *control -> other_angle_feedback;
            }
            pid_ref = PID_Calc(&control -> angle_pid, pid_ref, pid_measure);
        }
        else if(motor -> config.loop_type & SPEED_LOOP)
        {
            if(config -> speed_feedback_source == MOTOR_FEED)
            {
                pid_measure = measure -> speed;
            }
            else
            {
                pid_measure = *control -> other_speed_feedback;
            }
            pid_ref = PID_Calc(&control -> speed_pid, pid_ref, pid_measure);
        }

        set = (uint16_t)pid_ref;
        group_num = motor -> group_num;
        motor_num = motor -> motor_num;
        send_group[group_num].tx_buf[2 * motor_num] = (uint8_t)(set >> 8);
        send_group[group_num].tx_buf[2 * motor_num + 1] = (uint8_t)(set & 0xFF);

        if(motor -> motor_working_type == MOTOR_STOP)
        {
            memset(send_group[group_num].tx_buf + 2 * motor_num, 0, sizeof(uint16_t));
        }
    }

    for(size_t i = 0; i < 6; ++ i)
    {
        if(Motor_Enable[i])
        {
            CANSend(&send_group[i], 1);
        }
    }
}
void DJIMotorStop(DJIMotorInstance* motor)
{
    motor -> motor_working_type = MOTOR_STOP;
}

void DJIMotorStart(DJIMotorInstance* motor)
{
    motor -> motor_working_type = MOTOR_ENABLE;
}
